// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "config.h"

// 3.自定义节点类；
class Nazzle_Valves : public rclcpp::Node
{
public:
    Nazzle_Valves() : Node("nazzle_valves_node_cpp")
    {
        // 状态回调
        this->state_1 = this->create_subscription<std_msgs::msg::Int32>("valve_state_1", 10,
                                                                        std::bind(&Nazzle_Valves::valveCallback_1,
                                                                                  this, std::placeholders::_1));
        this->state_2 = this->create_subscription<std_msgs::msg::Int32>("valve_state_2", 10,
                                                                        std::bind(&Nazzle_Valves::valveCallback_2,
                                                                                  this, std::placeholders::_1));
    }

private:
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_1;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_2;
    int state_first = 0;
    int state_second = 0;

    void valveCallback_1(const std_msgs::msg::Int32::SharedPtr state_msg)
    {
        state_first = state_msg->data;
        switch (state_first)
        {
        case 0:
            SendData(Data, 0X01, CLOSE_1, 8);
            break;
        case 1:
            SendData(Data, 0X01, OPEN_1_1, 8);
            break;
        case 2:
            SendData(Data, 0X01, OPEN_1_2, 8);
            break;
        case 3:
            SendData(Data, 0X01, OPEN_1_3, 8);
            break;
        case 4:
            SendData(Data, 0X01, OPEN_1_4, 8);
            break;
        }
    }
    void valveCallback_2(const std_msgs::msg::Int32::SharedPtr state_msg)
    {
        state_second = state_msg->data;
        switch (state_second)
        {
        case 0:
            SendData(Data, 0X02, CLOSE_2, 8);
            break;
        case 1:
            SendData(Data, 0X02, OPEN_2_1, 8);
            break;
        case 2:
            SendData(Data, 0X02, OPEN_2_2, 8);
            break;
        case 3:
            SendData(Data, 0X02, OPEN_2_3, 8);
            break;
        case 4:
            SendData(Data, 0X02, OPEN_2_4, 8);
            break;
        }
    }
};

int main(int argc, char const *argv[])
{
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    // 初始化CAN设备
    Init_Can();
    // 4.调用spain函数，并传入节点对象指针；
    rclcpp::spin(std::make_shared<Nazzle_Valves>());
    // 5.资源释放
    rclcpp::shutdown();
    // 关闭CAN设备
    SendData(Data, 0X01, CLOSE_1, 8);
    SendData(Data, 0X02, CLOSE_2, 8);
    VCI_CloseDevice(device_type, device_index);
    return 0;
}
// CAN设备初始化函数
void Init_Can(void)
{
    // 初始化CAN设备
    device_type = VCI_USBCAN2; // 设备类型修改
    device_index = 0;          // 设备的索引
    can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

    if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
    {
        std::cout << "无法开启CAN设备" << std::endl;
        return;
    }
    else
    {
        std::cout << "设备开启,如需退出请按“CTRL + C”" << std::endl;
    }

    if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
    {
        std::cout << "获取设备信息失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
    // 设置CAN初始化配置
    init_config.AccCode = 0X00000000;
    init_config.AccMask = 0xFFFFFFFF;
    init_config.Filter = 2; // 只接受标准帧
    init_config.Mode = 0;
    init_config.Timing0 = 0x9F;
    init_config.Timing1 = 0xFF; // 波特率：10kps

    if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
    {
        std::cout << "初始化CAN参数失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }

    // 开始CAN通讯
    if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
    {
        std::cout << "开启CAN通道失败" << std::endl;
        VCI_CloseDevice(device_type, device_index);
        return;
    }
}

// 指令发送函数
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
    handle_obj.ID = id;
    handle_obj.SendType = 1; // 单次发送
    handle_obj.RemoteFlag = 0;
    handle_obj.ExternFlag = 0;
    handle_obj.DataLen = dataLen;
    for (int i = 0; i < dataLen; i++)
    {
        handle_obj.Data[i] = data[i];
    }
    if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
    {
    }
    else
    {
        std::cout << "初始化错误！" << std::endl;
    }
}